﻿#include <iostream>
#include <stdexcept>
#include <pcl/impl/point_types.hpp>
#include <stdio.h>
#include <fstream>
#include <iostream>
#include <string>
#include <liblas/liblas.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/impl/point_types.hpp> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <memory>
#include <pcl/io/pcd_io.h>
 
#include <utility>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>//模型定义头文件
#include <pcl/sample_consensus/model_types.h>//随机参数估计方法头文件
#include <pcl/features/normal_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h> // 拟合直线
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/common/transforms.h>	// 点云旋转
#include <pcl/filters/model_outlier_removal.h>	// 消除离群点
#include <pcl/sample_consensus/sac_model_cylinder.h>	//拟合圆柱体
#include <pcl/features/boundary.h>	// 点云边界拟合
#include <pcl/filters/conditional_removal.h>
#include <set>
#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include <pcl/sample_consensus/sac_model_normal_plane.h>	// 法线估计平面

#include "OverUnderExcavated.h"


using namespace Eigen;


// 自定义比较函数，按照 XYZ 坐标进行比较
struct ComparePointXYZ {
	bool operator()(const pcl::PointXYZ& lhs, const pcl::PointXYZ& rhs) const {
		// 首先按照 X 坐标比较
		if (lhs.x != rhs.x) {
			return lhs.x < rhs.x;
		}
		// 如果 X 坐标相等，则比较 Y 坐标
		else if (lhs.y != rhs.y) {
			return lhs.y < rhs.y;
		}
		// 如果 X 和 Y 坐标都相等，则比较 Z 坐标
		else {
			return lhs.z < rhs.z;
		}
	}
};

pcl::PointCloud<pcl::PointXYZ>::Ptr getPCFromLAS() {
	/* 读取文件 */
	std::ifstream ifs;
	// ifs.open("D:\\Project\\pointcloud\\data\\segmant\\segment_56_1_1.las", std::ios::in | std::ios::binary);
	// ifs.open("D:\\Project\\pointcloud\\tunnel.las", std::ios::in | std::ios::binary);
	ifs.open("D:\\Project\\pointcloud\\res2.las", std::ios::in | std::ios::binary);
	// ifs.open("D:\\Project\\pointcloud\\XYZ.las", std::ios::in | std::ios::binary);

	/* 使用liblas读取las文件 */
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs);
	liblas::Header const& header = reader.GetHeader();

	/* 输出 LAS 文件的版本号的次版本号 */
	uint8_t minorVersion = header.GetVersionMinor();
	std::cout << "Minor Version: " << +minorVersion << std::endl;

	/* 定义点云对象 */
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = header.GetPointRecordsCount();
	cloud->height = 1;
	cloud->is_dense = false;

	/* 输出测试信息 */
	std::cout << typeid(cloud->points).name() << std::endl;
	std::cout << header.GetPointRecordsCount() << std::endl;

	/* 将读取的las文件赋值给PCL的点云对象 */
	for (int i = 0; reader.ReadNextPoint(); i++) {
		const liblas::Point& point = reader.GetPoint();
		pcl::PointXYZ p(point.GetX(), point.GetY(), point.GetZ());
		cloud->points.push_back(p);
	}
	return cloud;
}

void visualization(pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("wall"));
	pcl::PointCloud<pcl::PointXYZ>::ConstPtr ConstPC = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(*pc);
	viewer->addPointCloud(ConstPC, "ewall");
	viewer->setBackgroundColor(0, 0, 0);
	viewer->initCameraParameters();
	viewer->spin();
}

pcl::ModelCoefficients::Ptr fitPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float maxDistance, int iterationNum, pcl::PointCloud<pcl::PointXYZ>::Ptr plane, pcl::PointCloud<pcl::PointXYZ>::Ptr outliers) {

	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ>* ransac = new pcl::RandomSampleConsensus<pcl::PointXYZ>(model_plane);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	Eigen::VectorXf coef;

	ransac->setDistanceThreshold(maxDistance);	//内点到模型的最大距离
	ransac->setMaxIterations(iterationNum);		//最大迭代次数
	ransac->computeModel();				//直线拟合
	ransac->getModelCoefficients(coef);


	// 构造pcl::ModelCoefficients
	coefficients->values.resize(coef.size());
	for (int i = 0; i < coef.size(); ++i) {
		coefficients->values[i] = coef[i];
	}

	// 根据索引提取内点
	std::vector<int> inliers;
	ransac->getInliers(inliers);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *plane);

	// 提取外点
	for (size_t i = 0; i < cloud->size(); ++i) {
		if (std::find(inliers.begin(), inliers.end(), i) == inliers.end()) {
			outliers->push_back((*cloud)[i]);
		}
	}

	return coefficients;
}

bool saveLAS(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::string& filePath) {
	std::ofstream ofs;
	ofs.open(filePath, std::ios::out | std::ios::binary);

	liblas::WriterFactory f;
	liblas::Header header;

	header.SetDataFormatId(liblas::ePointFormat1); // Time only

	liblas::Writer writer(ofs, header);


	// 将 PCL 的点云数据转换为 libLAS 的点云数据并保存
	for (int i=0; i<cloud->points.size(); i++)
	{
		/*
		if (cloud->points[i].x < 0)
			cloud->points[i].x = -cloud->points[i].x;
		if (cloud->points[i].y < 0)
			cloud->points[i].y = -cloud->points[i].y; 
		if (cloud->points[i].z < 0)
			cloud->points[i].z = -cloud->points[i].z;
		*/

		liblas::Point laspoint(&header);
		laspoint.SetCoordinates(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
		writer.WritePoint(laspoint);
	}
	return true;
}

bool comparePoints(const pcl::PointXYZ& point1, const pcl::PointXYZ& point2) {
	return point1.x == point2.x && point1.y == point2.y && point1.z == point2.z;
}

bool compareX(const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
	return a.x < b.x;
}

bool compareY(const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
	return a.y < b.y;
}

bool compareYRGB(const pcl::PointXYZRGB& a, const pcl::PointXYZRGB& b) {
	return a.y < b.y;
}

bool compareYZ(const pcl::PointXYZRGB& a, const pcl::PointXYZRGB& b) {
	return a.y + a.z < b.y + b.z;
}

bool compareZ(const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
	return a.z < b.z;
}
bool compareZRGB(const pcl::PointXYZRGB& a, const pcl::PointXYZRGB& b) {
	return a.z < b.z;
}
bool compareZ_negative(const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
	return a.z > b.z;
}

void deduplication(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) {
	std::set<pcl::PointXYZ, ComparePointXYZ> pointSet;

	for (const auto& point : cloud->points) {
		pointSet.insert(point);
	}


	cloud->clear();

	for (const auto& point : pointSet) {
		cloud->points.push_back(point);
	}
}

pcl::PointCloud<pcl::PointXYZ>::Ptr projection(pcl::PointCloud<pcl::PointXYZ>::ConstPtr origianlPC, int a, int b, int c, int d) {
	
	pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients());
	coefficient->values.resize(4);
	coefficient->values[0] = a;
	coefficient->values[1] = b;
	coefficient->values[2] = c;
	coefficient->values[3] = d;

	pcl::PointCloud<pcl::PointXYZ>::Ptr projPC(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(origianlPC);
	proj.setModelCoefficients(coefficient);
	proj.filter(*projPC);
	cout << "去重前投影点个数：" << projPC->size() << endl;

	// 去重
	deduplication(projPC);

	cout << "去重后投影点个数：" << projPC->size() << endl;

	return projPC;
}

void convexHull(pcl::PointCloud<pcl::PointXYZ>::Ptr originalPC, pcl::PointCloud<pcl::PointXYZ>::Ptr& res) {
	pcl::ConvexHull<pcl::PointXYZ> hull;
	hull.setInputCloud(originalPC);
	hull.setDimension(2);

	std::vector<pcl::Vertices> polygons;
	hull.reconstruct(*res, polygons);

	if (false) {
		// 可视化
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
		viewer->addPointCloud(res, "surface");
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "surface");
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "surface");
		viewer->addPolygon<pcl::PointXYZ>(res, 0, 0.069 * 255, 0.2 * 255, "baclview");
		viewer->spin();

	}
}

void concaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr originalPC, pcl::PointCloud<pcl::PointXYZ>::Ptr& res) {
	pcl::ConcaveHull<pcl::PointXYZ> chull;
	chull.setInputCloud(originalPC);
	chull.setAlpha(1);
	chull.reconstruct(*res);

	/*
	cout << "凹包点的个数:  " << res->size() << endl;

	for (int i = 0; i < res->size(); i++) {
		cout << res->points[i].x << "  " << res->points[i].y << endl;
	}
	*/

	if (false) {
		// 可视化
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
		viewer->addPointCloud(res, "surface");
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "surface");
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "surface");
		viewer->addPolygon<pcl::PointXYZ>(res, 0, 0.069 * 255, 0.2 * 255, "baclview");

		viewer->spin();

	}
}

std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr>  getBoundaryLine(
	pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
	pcl::ModelCoefficients::Ptr line1,
	pcl::ModelCoefficients::Ptr line2
) {
	//内点点云合并
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line1(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line2(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr outliers(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr middlePC(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr total(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointIndices::Ptr inliersPtr(new pcl::PointIndices);
	Eigen::VectorXf coef;

	pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line1(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ>* ransac1 = new pcl::RandomSampleConsensus<pcl::PointXYZ>(model_line1);

	ransac1->setDistanceThreshold(0.5);	//内点到模型的最大距离
	ransac1->setMaxIterations(100);		//最大迭代次数
	ransac1->computeModel();				//直线拟合
	ransac1->getModelCoefficients(coef);

	// 构造pcl::ModelCoefficients
	line1->values.resize(coef.size());
	for (int i = 0; i < coef.size(); ++i) {
		line1->values[i] = coef[i];
	}

	// 根据索引提取内点
	std::vector<int> inliers;
	ransac1->getInliers(inliers);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_line1);

	// 提取外点
	for (size_t i = 0; i < cloud->size(); ++i) {
		if (std::find(inliers.begin(), inliers.end(), i) == inliers.end()) {
			outliers->push_back((*cloud)[i]);
		}
	}
	
	pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line2(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(outliers));
	pcl::RandomSampleConsensus<pcl::PointXYZ>* ransac2 = new pcl::RandomSampleConsensus<pcl::PointXYZ>(model_line2);
	
	ransac2->setDistanceThreshold(0.5);	//内点到模型的最大距离
	ransac2->setMaxIterations(100);		//最大迭代次数
	ransac2->computeModel();				//直线拟合
	ransac2->getModelCoefficients(coef);

	// 构造pcl::ModelCoefficients
	line2->values.resize(coef.size());
	for (int i = 0; i < coef.size(); ++i) {
		line2->values[i] = coef[i];
	}
	//根据索引提取内点
	std::vector<int> inliers2;
	ransac2->getInliers(inliers2);
	pcl::copyPointCloud<pcl::PointXYZ>(*outliers, inliers2, *cloud_line2);


	return std::make_pair(cloud_line1, cloud_line2);
}

float getAxisXOY(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line2, pcl::ModelCoefficients::Ptr coefficients) {

	pcl::PointCloud<pcl::PointXYZ>::Ptr middlePC(new pcl::PointCloud<pcl::PointXYZ>), total(new pcl::PointCloud<pcl::PointXYZ>);

	// 计算轴线上的点
	int minNum = cloud_line1->size() < cloud_line2->size() ? cloud_line1->size() : cloud_line2->size();
	float x, y, sum = 0;

	std::sort(cloud_line1->points.begin(), cloud_line1->points.end(), compareX);
	std::sort(cloud_line2->points.begin(), cloud_line2->points.end(), compareX);

	for (int i = 0; i < minNum; i++) {
		x = (cloud_line1->points[i].x + cloud_line2->points[i].x) / 2;
		y = (cloud_line1->points[i].y + cloud_line2->points[i].y) / 2;
		middlePC->push_back(pcl::PointXYZ(x, y, 0));
		sum += sqrt(pow(cloud_line1->points[i].x - cloud_line2->points[i].x, 2) + pow(cloud_line1->points[i].y - cloud_line2->points[i].y, 2));
	}

	// 可视化提取的隧道边界的点
	if (false) {
		*total = *cloud_line1 + *cloud_line2 + *middlePC;
		visualization(total);
	}

	// 拟合轴线
	pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line3(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(middlePC));
	pcl::RandomSampleConsensus<pcl::PointXYZ>* ransac3 = new pcl::RandomSampleConsensus<pcl::PointXYZ>(model_line3);

	ransac3->setDistanceThreshold(0.05);	//内点到模型的最大距离
	ransac3->setMaxIterations(100);		//最大迭代次数
	ransac3->computeModel();				//直线拟合
	Eigen::VectorXf coef;
	ransac3->getModelCoefficients(coef);


	// 构造pcl::ModelCoefficients
	coefficients->values.resize(coef.size());
	for (int i = 0; i < coef.size(); ++i) {
		coefficients->values[i] = coef[i];
	}

	std::cout << "XOY平面轴线参数: ";
	for (size_t i = 0; i < coefficients->values.size(); ++i) {
		std::cout << std::fixed << coefficients->values[i] << " ";
	}
	cout << endl;

	return sum / minNum;
}

void getAxisXOZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line2, float width, pcl::ModelCoefficients::Ptr coefficients) {
	// 得到上边界线
	pcl::PointCloud<pcl::PointXYZ>::Ptr top_cloud, total(new pcl::PointCloud<pcl::PointXYZ>);
	float z_line1 = 0, z_line2 = 0, half_width=width/2;

	for (int i=0; i < cloud_line1->size(); ++i)
		z_line1 += cloud_line1->points[i].z;
	for (int i=0; i < cloud_line2->size(); ++i)
		z_line2 += cloud_line2->points[i].z;

	if (z_line1 > z_line2)
		pcl::copyPoint(cloud_line1, top_cloud);
	else
		pcl::copyPoint(cloud_line2, top_cloud);

	for (int i=0; i < top_cloud->size(); ++i)
		top_cloud->points[i].z -= half_width;

	cout << "拟合XOZ平面轴线时的宽度：" << half_width << endl;

	std::sort(cloud_line1->points.begin(), cloud_line1->points.end(), compareZ);
	std::sort(cloud_line2->points.begin(), cloud_line2->points.end(), compareZ);
	std::sort(top_cloud->points.begin(), top_cloud->points.end(), compareZ);

	cout << "line1 " << cloud_line1->points[0].z << "  " << cloud_line1->points[cloud_line1->points.size()-1].z << endl;
	cout << "line2 " << cloud_line2->points[0].z << "  " << cloud_line2->points[cloud_line2->points.size() - 1].z << endl;
	cout << "top_cloud " << top_cloud->points[0].z << "  " << top_cloud->points[top_cloud->points.size() - 1].z << endl;

	if (false) {
		// 可视化
		pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1(cloud_line1, 255, 0, 0); // 红色
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud_line2, 0, 0, 255); // 蓝色
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color3(top_cloud, 0, 255, 0); // 绿色

		viewer->addPointCloud(cloud_line1, color1, "cloud1");
		viewer->addPointCloud(cloud_line2, color2, "cloud2");
		viewer->addPointCloud(top_cloud, color3, "cloud3");
		viewer->spin();

	}
	// 拟合上边界点
	pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(top_cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ>* ransac = new pcl::RandomSampleConsensus<pcl::PointXYZ>(model_line);

	ransac->setDistanceThreshold(0.05);	//内点到模型的最大距离
	ransac->setMaxIterations(100);		//最大迭代次数
	ransac->computeModel();				//直线拟合
	Eigen::VectorXf coef;
	ransac->getModelCoefficients(coef);

	coefficients->values.resize(coef.size());
	cout << "XOZ平面的轴线参数:   ";
	for (int i = 0; i < coef.size(); ++i) {
		coefficients->values[i] = coef[i];
		cout << std::fixed << coef[i] << "   ";
	}
	cout << endl;
}

void rotate_(pcl::PointCloud<pcl::PointXYZ>::Ptr& originalPC, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointCloud<pcl::PointXYZ>::Ptr& res) {
	// 轴线的方向向量
	Eigen::Vector3f axis_direction(coefficients->values[3], coefficients->values[4], coefficients->values[5]);

	// 计算旋转角度
	Eigen::Vector3f target_axis(1, 0, 0);
	axis_direction.normalize();
	float angle = std::acos(target_axis.dot(axis_direction));

	// 计算旋转轴
	Eigen::Vector3f rotation_axis = axis_direction.cross(target_axis);
	rotation_axis.normalize();

	// 创建旋转矩阵
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.rotate(Eigen::AngleAxisf(angle, rotation_axis));

	// 应用旋转变换到点云上
	pcl::PointCloud<pcl::PointXYZ>::Ptr total(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*originalPC, *res, transform);

	cout << "原始点云的点数：  " << originalPC->points.size() << endl;
	cout << "旋转后的点云点数：" << res->points.size() << endl;


}

void rotate(pcl::PointCloud<pcl::PointXYZ>::Ptr& originalPC, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointCloud<pcl::PointXYZ>::Ptr& res) {
	/* 要得到旋转之后轴线的参数 */
	pcl::ModelCoefficients::Ptr& axis = coefficients;

	// 提取轴线上的点和方向向量
	Vector3d axisVector(axis->values[3], axis->values[4], axis->values[5]);
	pcl::PointXYZ pointOnAxis(axis->values[0], axis->values[1], axis->values[2]);

	// 计算将 axisVector 旋转到与 x 轴重合所需的旋转角度
	double angle = std::acos(axisVector.normalized().dot(Vector3d::UnitX()));

	// 计算旋转轴
	Vector3d axisOfRotation = axisVector.cross(Vector3d::UnitX()).normalized();

	// 通过旋转矩阵来旋转轴线到与 x 轴重合
	AngleAxisd rotation(angle, axisOfRotation);
	Matrix3d rotationMatrix = rotation.toRotationMatrix();

	// 构建变换矩阵
	Affine3f transform = Affine3f::Identity();
	transform.rotate(rotationMatrix.cast<float>());

	// 应用变换矩阵到点云数据和轴线上的点
	pcl::transformPointCloud(*originalPC, *res, transform);

	// 计算旋转后的轴线方向向量和点的坐标
	Vector3d rotatedAxisVector = rotationMatrix * axisVector;
	pointOnAxis = pcl::transformPoint(pointOnAxis, transform);

	// 构造旋转之后轴线的参数
	axis->values[0] = pointOnAxis.x;
	axis->values[1] = pointOnAxis.y;
	axis->values[2] = pointOnAxis.z;
	axis->values[3] = rotatedAxisVector.transpose()[0];
	axis->values[4] = rotatedAxisVector.transpose()[1];
	axis->values[5] = rotatedAxisVector.transpose()[2];


	cout << "旋转之后轴线的参数: " << pointOnAxis.x << " " << pointOnAxis.y << " " << pointOnAxis.z << " " << rotatedAxisVector.transpose() << endl;
	cout << "原始点云的点数：  " << originalPC->points.size() << endl;
	cout << "旋转后的点云点数：" << res->points.size() << endl;
}

void translateToXAxis(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::ModelCoefficients::Ptr& coefficients) {
	/* 在Y轴方向平移 */
	float distance = coefficients->values[1];

	// 定义仿射变化矩阵
	Eigen::Affine3f  translation = Eigen::Affine3f::Identity();
	translation.translation() << 0, -distance, 0;

	pcl::transformPointCloud(*cloud, *cloud, translation);
	
}

pcl::PointCloud<pcl::PointXYZ>::Ptr slice(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float x, float threshold) {

	pcl::PointCloud<pcl::PointXYZ>::Ptr nearby_points(new pcl::PointCloud<pcl::PointXYZ>);
	for (size_t i = 0; i < cloud->size(); ++i)
	{
		if (std::abs(cloud->points[i].x - x) < threshold)
		{
			nearby_points->push_back(cloud->points[i]);
		}
	}

	return nearby_points;
}

void getAreaBoundaryPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& temp, pcl::PointCloud<pcl::PointXYZ>::Ptr& res ) {
	float y = 0, z = 0;
	for (auto p : temp->points) {
		y += p.y;
		z += p.z;
	}
	res->push_back(pcl::PointXYZ(0, y / temp->points.size(), z / temp->points.size()));
}

// 最近邻插值函数实现
pcl::PointCloud<pcl::PointXYZ>::Ptr nearestNeighborInterpolation(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, int num_interp_points)
{
	// 创建KdTree对象进行最近邻搜索
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(inputCloud);

	// 新点云，用于存储插值后的点
	pcl::PointCloud<pcl::PointXYZ>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZ>);

	// 对每个点进行最近邻搜索并插值
	for (size_t i = 0; i < inputCloud->points.size(); ++i) {
		std::vector<int> pointIdxNKNSearch(1);
		std::vector<float> pointNKNSquaredDistance(1);

		// 寻找最近邻点
		if (kdtree->nearestKSearch(inputCloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
			// 在原始点和其最近邻点之间插入一个点
			for (int j = 0; j < num_interp_points; ++j) {
				float ratio = static_cast<float>(j + 1) / (num_interp_points + 1);
				pcl::PointXYZ newPoint;
				newPoint.x = inputCloud->points[i].x + ratio * (inputCloud->points[pointIdxNKNSearch[1]].x - inputCloud->points[i].x);
				newPoint.y = inputCloud->points[i].y + ratio * (inputCloud->points[pointIdxNKNSearch[1]].y - inputCloud->points[i].y);
				newPoint.z = inputCloud->points[i].z + ratio * (inputCloud->points[pointIdxNKNSearch[1]].z - inputCloud->points[i].z);
				outputCloud->push_back(newPoint);
			}
		}
		outputCloud->push_back(inputCloud->points[i]);
	}

	deduplication(outputCloud);

	*outputCloud = *outputCloud + *inputCloud;

	return outputCloud;
}

void getBoundaryPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr res) {
	int span = 100, i=0;
	pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>), interpolatePoint(new pcl::PointCloud<pcl::PointXYZ>);

	// 按Y坐标排序
	std::sort(cloud->points.begin(), cloud->end(), compareY);
	int num = std::ceil(cloud->points.size() / span);
	// 检测num的值，其值最少是2
	if (num < 2) {
		num = 2;
	}

	cout << "每一跨度 " << num << "个点, " << "一共" << span << "/" << (span + 1) << "个跨度" << endl;

	for (auto point : *cloud) {
		temp->push_back(point);
		i++;

		if (i % num == 0) {
			// 得到边界点
			getAreaBoundaryPoint(temp, res);
			temp->clear();
		}
	}

	// 处理剩下的点
	if (temp->points.size()) {
		getAreaBoundaryPoint(temp, res);
	}
}

void getBoundary(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr res) {

	// 计算法向量
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud<pcl::Normal>::Ptr  normals(new  pcl::PointCloud<pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
	normalEstimation.setInputCloud(cloud);
	normalEstimation.setSearchMethod(tree);
	normalEstimation.setRadiusSearch(10);  // 法向量的半径 
	normalEstimation.compute(*normals);

	// 计算边界
	pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>); 
	boundaries->resize(cloud->size()); 
	pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundary_estimation;
	boundary_estimation.setInputCloud(cloud); 
	boundary_estimation.setInputNormals(normals);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree_ptr(new pcl::search::KdTree<pcl::PointXYZ>);
	boundary_estimation.setSearchMethod(kdtree_ptr); //设置搜寻k近邻的方式
	boundary_estimation.setKSearch(50); //设置k近邻数量
	boundary_estimation.setAngleThreshold(M_PI * 0.85); //设置角度阈值，大于阈值为边界
	boundary_estimation.compute(*boundaries);


	// 可视化与构造res点云
	bool flag;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_visual(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud_visual->resize(cloud->size());
	for (size_t i = 0; i < cloud->size(); i++)
	{
		flag = true;
		cloud_visual->points[i].x = cloud->points[i].x;
		cloud_visual->points[i].y = cloud->points[i].y;
		cloud_visual->points[i].z = cloud->points[i].z;
		if (boundaries->points[i].boundary_point != 0)
		{
			cloud_visual->points[i].r = 255;
			cloud_visual->points[i].g = 0;
			cloud_visual->points[i].b = 0;
			cloud_boundary->push_back(cloud_visual->points[i]);
			res->push_back(pcl::PointXYZ(cloud_visual->points[i].x, cloud_visual->points[i].y, cloud_visual->points[i].z));
		}
		else
		{
			cloud_visual->points[i].r = 0;
			cloud_visual->points[i].g = 255;
			cloud_visual->points[i].b = 0;
		}
	}

	cout << "原始点" << cloud->points.size() << endl;
	cout << "边界点" << cloud_boundary->points.size() << endl;

	pcl::visualization::PCLVisualizer::Ptr viewer1(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));
	viewer1->addPointCloud(cloud_visual, "cloud");
	viewer1->spin();


	int i;
	std::sort(res->begin(), res->end(), compareY);
	for (i = 0; i < res->points.size(); i++) {
		if (res->points[i].y > 0)
			break;
	}
	std::sort(res->points.begin(), res->points.begin() + i, compareZ);
	std::sort(res->points.begin()+i, res->points.end(), compareZ_negative);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_(new pcl::visualization::PCLVisualizer);
	viewer_->addPointCloud(res);
	viewer_->addPolygon<pcl::PointXYZ>(res, 0, 0.069 * 255, 0.2 * 255, "baclview");

	viewer_->spin();

}

pcl::PointCloud<pcl::PointXYZ>::Ptr deleteObjOnGround(pcl::PointCloud<pcl::PointXYZ>::Ptr plane, pcl::PointCloud<pcl::PointXYZ>::Ptr wall, pcl::ModelCoefficients::Ptr coefficients) {
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr res(new pcl::PointCloud<pcl::PointXYZ>);

	// 去除隧道中两个长方形	(最优的阈值：60246.9)
	float a, b, c, d, distance, Q, threshold = 60247;
	a = coefficients->values[0];
	b = coefficients->values[1];
	c = coefficients->values[2];
	d = coefficients->values[3];

	Q = sqrt(a * a + b * b + c * c);


	float max = 0, min = std::numeric_limits<float>::max();

	for (size_t i = 0; i < wall->points.size(); i++) {
		distance = fabs(a * wall->points[i].x + b * wall->points[i].y + c * wall->points[i].z) / Q;
		if (distance > max) { max = distance; }
		if (distance < min) { min = distance; }
		if (distance > threshold)
			res->push_back(wall->points[i]);
	}

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1(res, 255, 255, 255);
	viewer->addPointCloud(res, color1, "cloud1");
	viewer->spin();

	return res;
}


std::pair<float, float> getCircleCoordinateYOZ(pcl::ModelCoefficients::Ptr line) {
	float x0 = line->values[0], y0 = line->values[1], z0 = line->values[2];
	float a = line->values[3], b = line->values[4], c = line->values[5];
	float y, z;

	y = y0 - (b * x0 / a);
	z = z0 - (c * x0 / a);

	return std::make_pair(y, z);
} 


int orthogonalCoordinateSystem() {
	try {
		bool isvisualize = false;

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOrign = getPCFromLAS();
		pcl::PointCloud<pcl::PointXYZ>::Ptr XOYPC;
		pcl::PointCloud<pcl::PointXYZ>::Ptr XOZPC;
		pcl::PointCloud<pcl::PointXYZ>::Ptr resConcave(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr wall(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr resConvex(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr rotateXOYRes(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr rotateXOZRes(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::ModelCoefficients::Ptr coefficients_ground(new pcl::ModelCoefficients);
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
		pcl::ModelCoefficients::Ptr coefficients_line1(new pcl::ModelCoefficients);
		pcl::ModelCoefficients::Ptr coefficients_line2(new pcl::ModelCoefficients);
		pcl::ModelCoefficients::Ptr coefficients_axis(new pcl::ModelCoefficients);
		float width;

		if (isvisualize)
			visualization(cloudOrign);


		// 除去地面
		coefficients = fitPlane(cloudOrign, 0.5, 100, plane, wall);
		if (isvisualize) {
			visualization(plane);
			visualization(wall);
		}
		cout << "地面的点云数量:  " << plane->points.size() << endl;
		cout << "去除地面后的点云数量:  " << wall->points.size() << endl;

		// 除去地面上的物体
		wall = deleteObjOnGround(plane, wall, coefficients);


		// 投射到XOY平面上
		XOYPC = projection(wall, 0, 0, 1, 0);

		if (isvisualize)
			visualization(XOYPC);

		// 凸包算法
		convexHull(XOYPC, resConvex);

		// 凹包算法
		concaveHull(XOYPC, resConcave);

		// 得到投影的上下界线
		auto boundaries = getBoundaryLine(resConcave, coefficients_line1, coefficients_line2);

		// 可视化XOY面投影的边界线
		if (isvisualize) {
			pcl::visualization::PCLVisualizer::Ptr viwer_boundary(new pcl::visualization::PCLVisualizer("viewer_boundary"));
			viwer_boundary->addPointCloud(XOYPC, "XOYPC");
			viwer_boundary->addPointCloud(boundaries.first, "boundary1");
			viwer_boundary->addPointCloud(boundaries.second, "boundary2");
			viwer_boundary->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "boundary1");
			viwer_boundary->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "boundary2");
			viwer_boundary->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "boundary1");
			viwer_boundary->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "boundary2");
			viwer_boundary->spin();
		}

		// 得到轴线
		width = getAxisXOY(boundaries.first, boundaries.second, coefficients);
		cout << "通过点点距离算出来的隧道宽度：" << width << endl;

		// 旋转XOY平面得，并到旋转之后轴线的参数
		rotate(wall, coefficients, rotateXOYRes);

		// 将旋转后的点云平移到将轴线与x轴重合
		translateToXAxis(rotateXOYRes, coefficients);
		
		if (isvisualize)
			visualization(rotateXOYRes);


		// 相同的方法计算XOZ平面的轴线并旋转

		XOZPC = projection(rotateXOYRes, 0, 1, 0, 0);

		if (isvisualize)
			visualization(XOZPC);

		convexHull(XOZPC, resConvex);
		concaveHull(XOZPC, resConcave);

		// 得到上下边界线
		boundaries = getBoundaryLine(resConcave, coefficients_line1, coefficients_line2);

		// 得到轴线
		getAxisXOZ(boundaries.first, boundaries.second, width, coefficients_axis);

		if (isvisualize) {
			pcl::visualization::PCLVisualizer::Ptr viewer_(new pcl::visualization::PCLVisualizer("Viewer"));
			viewer_->addPointCloud(XOZPC);
			viewer_->addLine(*coefficients_axis, "");
			viewer_->spin();
		}

		cout << "XOZ旋转之前点的数量 :" << rotateXOYRes->points.size() << endl;

		rotate(rotateXOYRes, coefficients_axis, rotateXOZRes);	/* 旋转有问题：有重叠的点 */
		cout << "XOZ旋转之后点的数量 :" << rotateXOZRes->points.size() << endl;

		deduplication(rotateXOZRes);
		cout << "XOZ去重之后点的数量 :" << rotateXOZRes->points.size() << endl;


		/* 第二个阶段 进行断面投影并求得圆心坐标 */
		isvisualize = false;

		// 分割隧道断面
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rotateXOZRes, nearby_points, YOZPC;
		pcl::PointCloud<pcl::PointXYZ>::Ptr res_boundary_point(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr res(new pcl::PointCloud<pcl::PointXYZ>);

		if (isvisualize)
			visualization(cloud);

		nearby_points = slice(cloud, (cloud->points[0].x + cloud->points[cloud->points.size()-1].x)/2 , 0.1);	/* 有问题: 选取点坐标不在x轴的范围内 */
		if(true)
			visualization(nearby_points);

		// 得到圆心的坐标
		auto circleCoordinate = getCircleCoordinateYOZ(coefficients_axis);
		cout << "断面投影到XOZ平面的圆心坐标是：(0, " << circleCoordinate.first << ", " << circleCoordinate.second << ")" << endl;

		// 投影断面到YOZ平面
		YOZPC = projection(nearby_points, 1, 0, 0, 0);
		
		if (isvisualize)
			visualization(YOZPC);

		// 得到边界点
		getBoundaryPoint(YOZPC, res_boundary_point);
		
		// 对边界点进行插值
		// res_boundary_point = nearestNeighborInterpolation(res_boundary_point, 5);
		// visualization(res_boundary_point);

		getBoundary(res_boundary_point, res);

		// 可视化边界点与圆心
		pcl::visualization::PCLVisualizer::Ptr viewer_YOZ(new pcl::visualization::PCLVisualizer("viewerYOX"));
		viewer_YOZ->addPointCloud(res);
		viewer_YOZ->addSphere(pcl::PointXYZ(0, circleCoordinate.first, circleCoordinate.second), 0.1, 0, 0, 1);
		viewer_YOZ->spin();
		
		// 计算轮廓上的点到圆心的平均距离  6.79   6.8
		float sum = 0;
		for (auto point : res->points) {
			float dis = sqrt(pow(point.y - circleCoordinate.first, 2) + pow(point.z - circleCoordinate.second, 2));
			cout << dis << endl;
			sum += dis;
		}
		cout << "平均距离: " << sum / res->points.size() << endl;
		cout << "圆心坐标: " << circleCoordinate.first << "  " << circleCoordinate.second << endl;

		// 构造超欠挖属性
		OverUnderExcavated* overUnderExcavater = new OverUnderExcavated(res, circleCoordinate.first, circleCoordinate.second, 40, 90, 6.7);
		overUnderExcavater->computeOverUnder();
		overUnderExcavater->computeIntersection();
		overUnderExcavater->computeOverUnderArea();
		overUnderExcavater->computeOverUnderEarthVolume(0.1);

	}
	catch (std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return 0;
	}
}


int main() {
	orthogonalCoordinateSystem();

	return 0 ;

}
